/*
 * kernel.c: Kernel main (entry) function
 *
 */
#include <exports.h>
#include <main.h>
#include <sys_handler.h>

#include <arm/psr.h>
#include <arm/exception.h>
#include <arm/interrupt.h>
#include <arm/timer.h>
#include <kernel.h>
volatile unsigned long ticks;
uint32_t global_data;
int kernel_sp = 0;
unsigned int uboot_irq[3];

int kmain(int argc, char** argv, uint32_t table)
{
	app_startup(); /* bss is valid after this point */
	global_data = table;

	/* Make irq stack point to stack base */
	if(setup_irq(uboot_irq) < 0)  {
	        puts("Timer setup error\n");
        	return BAD_CODE;
	}

	/* Add your code here */
        /* location of SWI vector */ 
	  unsigned long *vtrAdd = (unsigned long*)0x8;
       	  unsigned long *irqAdd  = (unsigned long*)0x18;
		
	  signed long *offset;
	  unsigned long *swiAdd;
	  unsigned long *ptr1;
	  unsigned long *ptr2;
	  unsigned long save1;
	  unsigned long save2;
	 
	  int rtnVal = 0; 
	 
	  /* get offset (positive case) */
	  if (!((*vtrAdd - 0xe59ff000) & 0xfffff000))
	    offset = (unsigned long*)(*vtrAdd - 0xe59ff000);
	  /* negative case */ 
	  else if(!((*vtrAdd - 0x51ff000) & 0xfffff000))
	    offset = (unsigned long*)(0x0 - (*vtrAdd - 0xe51ff000));
	  /* unrecognized */
	  else
	    return 0x0badc0de;
	   
	  offset = (unsigned long*)(offset +0x4); /* offset + pc */
	  swiAdd = (unsigned long*)*offset;    /* get address of SWI handler */
	  ptr1 = (unsigned long *)swiAdd;
	  ptr2 = (unsigned long *)(swiAdd+0x1);
	  
	  /* saving original SWI handler */
	  save1 = (unsigned long)*ptr1;
	  save2 = (unsigned long)*ptr2;
	  
	  
	  
	  
	  /* wiring in our SWI handler */
	  *ptr1 = 0xe51ff004;
	  *ptr2 = (unsigned long)SWI_handler;
	   
	  /* go to assembly kernel */
	  rtnVal = loadP(argc, argv);

	  
	  /* restore original SWI handler */
	  *ptr1 = save1;
	  *ptr2 = save2;
	
	  return rtnVal;
}

